#include "FrameGrammar.h"
#include <iostream>
using namespace std;


FrameGrammar::FrameGrammar(MIDIProcessor* mp)
{
	midi = mp;
	striking = false;
	instrument = 0;

	midi->changeInstrument(1);
	midi->setVolume(100);
}

/* inPosition
   Check that the mallet is within a valid area for note detection.
   If it isn't, the mallet is said to be "raised".
*/
bool FrameGrammar::inPosition(int x, int y, ImageProcessor* ip)
{
	int lSide = ((ip->getUpperLeft()->getX() + ip->getLowerLeft()->getX())/2);
	int rSide = ((ip->getUpperRight()->getX() + ip->getLowerRight()->getX())/2);

	int uSide = ((ip->getUpperLeft()->getY() + ip->getUpperRight()->getY())/2);
	int dSide = ((ip->getLowerLeft()->getY() + ip->getLowerRight()->getY())/2);

	if(x < lSide || x > rSide)
		return false;

	if(y < uSide || y > dSide)
		return false;

	return true;
}



FrameGrammar::~FrameGrammar(void)
{
	if(midi != 0)
		delete midi;
}

/* getNote
   Computes which note the mallet is positioned over
*/

int FrameGrammar::getNote(int x, int y, ImageProcessor* ip)
{
	int lSide = ((ip->getUpperLeft()->getX() + ip->getLowerLeft()->getX())/2);
	int rSide = ((ip->getUpperRight()->getX() + ip->getLowerRight()->getX())/2);
	int horizontal = rSide - lSide;

	// 8 notes
	if(x < lSide)
		return -1;
	else if(x >= lSide && x < lSide + horizontal / 8)
		return 60;
	else if(x >= lSide + horizontal / 8 && x < lSide + horizontal / 4)
		return 62;
	else if(x >= lSide + horizontal / 4 && x < lSide + horizontal * 3 / 8)
		return 64;
	else if(x >= lSide + horizontal * 3 / 8 && x < lSide + horizontal * 1 / 2)
		return 65;
	else if(x >= lSide + horizontal * 1 / 2 && x < lSide + horizontal * 5 / 8)
		return 67;
	else if(x >= lSide + horizontal * 5 / 8 && x < lSide + horizontal * 6 / 8)
		return 69;
	else if(x >= lSide + horizontal * 6 / 8 && x < lSide + horizontal * 7 / 8)
		return 71;
	else if(x >= lSide + horizontal * 7 / 8 && x < lSide + horizontal)
		return 72;
	else //if(x >= lSide + horizontal)
		return -1;
}

/*  getMid
    Computes the Y midpoint of the key pad at a given X value.
*/
int FrameGrammar::getMid(int x, ImageProcessor* ip)
{
	double slope1 = (ip->getUpperRight()->getY() - ip->getUpperLeft()->getY()) / (ip->getUpperRight()->getX() - ip->getUpperLeft()->getX());
	double slope2 = (ip->getLowerRight()->getY() - ip->getLowerLeft()->getY()) / (ip->getLowerRight()->getX() - ip->getLowerLeft()->getX());

	// average higher and lower slope
	double avg = (slope1 + slope2)/2.0;

	// get x offset
	x -= (ip->getUpperLeft()->getX() + ip->getLowerLeft()->getX()) / 2;

	// compute y along the slope
	int y = (int)(ip->getLowerLeft()->getY() + avg * x);

	return y;
}

/* processFrame
   Processes the next frame (given the image)
*/
void FrameGrammar::processFrame(ImageProcessor* ip)
{
	bool pedalLoweredToRaised = false;
	bool pedalRaisedToLowered = false;

	bool malletLoweredToRaised = false;
	bool malletRaisedToLowered = false;

	// detect instrument change					////// CHANGE VALUES
	int newInstrument = -1;

	if(ip->getRedBlob()->getSize() > ImageProcessor::PIXEL_THRESHOLD)
	{
		newInstrument = 1;
	}
	else if(ip->getGreenBlob()->getSize() > ImageProcessor::PIXEL_THRESHOLD)
	{
		newInstrument = 17;
	}
	else if(ip->getBlueBlob()->getSize() > ImageProcessor::PIXEL_THRESHOLD)
	{
		newInstrument = 41;
	}

	if(newInstrument != instrument && newInstrument != -1)
	{
		instrument = newInstrument;
		midi->changeInstrument(instrument);
	}

	// detect pedal state
	bool pedalNewState = false;

	// "pedal" is visible
	if(ip->getPedalBlob()->getSize() > ImageProcessor::PIXEL_THRESHOLD)
	{
		// compute the state based on the pedal/hand ratio
		pedalNewState = ip->getPedalLowered(ip->getPedalBlob()->getRatio());
	}

	if(pedalLowered != pedalNewState)
	{
		// pedal has been lowered
		if(pedalNewState && !pedalLowered)
			pedalRaisedToLowered = true;
		// pedal has been raised
		else
			pedalLoweredToRaised = true;

		pedalLowered = pedalNewState;
	}

	// detect mallet raised or lowered
	bool malletNewState = false;						////// CHANGE

	switch(instrument)
	{
		// red
		case 1:
		{
			// mallet is lowered
			if(ip->getRedBlob()->getCOMY() < getMid(ip->getRedBlob()->getCOMX(), ip) &&
				inPosition(ip->getRedBlob()->getCOMX(), ip->getRedBlob()->getCOMY(), ip))
				malletNewState = true;

			break;
		}

		// green
		case 17:
		{
			// mallet is lowered
			if(ip->getGreenBlob()->getCOMY() < getMid(ip->getGreenBlob()->getCOMX(), ip) &&
				inPosition(ip->getGreenBlob()->getCOMX(), ip->getGreenBlob()->getCOMY(), ip))
				malletNewState = true;

			break;
		}

		// blue
		case 41:
		{
			// mallet is lowered
			if(ip->getBlueBlob()->getCOMY() < getMid(ip->getBlueBlob()->getCOMX(), ip) &&
				inPosition(ip->getBlueBlob()->getCOMX(), ip->getBlueBlob()->getCOMY(), ip))
				malletNewState = true;

			break;
		}

		default:
		{
			break;
		}
	}

	if(striking != malletNewState)
	{
		// mallet has been lowered
		if(malletNewState && !striking)
			malletRaisedToLowered = true;
		// mallet has been raised
		else
		{
			malletLoweredToRaised = true;
			lastNote = -1;
		}

		striking = malletNewState;
	}

	// now do note generation

	// first determine if there are new notes
	if(malletRaisedToLowered)
	{
		// determine note position by mallet
		int note = -1;									/////// CHANGE
		switch(newInstrument)
		{
			// red
			case 1:
			{
				note = getNote(ip->getRedBlob()->getCOMX(), ip->getRedBlob()->getCOMY(), ip);
				break;
			}

			// green
			case 17:
			{
				note = getNote(ip->getGreenBlob()->getCOMX(), ip->getGreenBlob()->getCOMY(), ip);
				break;
			}

			// blue
			case 41:
			{
				note = getNote(ip->getBlueBlob()->getCOMX(), ip->getBlueBlob()->getCOMY(), ip);
				break;
			}

			// shouldn't happen
			default:
			{
				break;
			}
		}

		// valid note
		if(note != -1)
		{
			midi->noteOn(note, 64);
			lastNote = note;
		}
	}

	// now determine if notes should be stopped

	// if mallet was lifted or pedal was lifted
	if((malletLoweredToRaised && !pedalLowered) || (pedalLoweredToRaised && !striking))
	{
		midi->stopAll();
	}
	else if(pedalLoweredToRaised && striking)
	{
		midi->stopAllExcept(lastNote);
	}
	else if(!pedalLowered && !striking)
	{
		midi->stopAll();
	}
}
